#include <boost/foreach.hpp>
#include <boost/array.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/asio.hpp>
#include <boost/multi_array.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/regex.hpp>
#include <cstdlib>
#include <iostream>
#include <fstream>

#include "protonos8.hpp"

using boost::asio::ip::udp;

namespace protonos8 { 

class server
{
public:
	server(boost::asio::io_service& io_service, short port)
		:
		io_service_(io_service),
		socket_(io_service, udp::endpoint(udp::v4(), port)),
		raw(boost::extents[8][0x40]),
		dac(boost::extents[8][8])
	{
		std::srand(static_cast<unsigned int>(std::time(NULL)));

		std::fill(raw.data(), raw.data()+raw.num_elements(), 0);
		std::fill(dac.data(), dac.data()+dac.num_elements(), 0);

		BOOST_FOREACH (boost::function< std::size_t (std::size_t) >& f, fid_dispatcher) {

			f = boost::bind(&server::unsupported, this, _1);
		}

		fid_dispatcher[fid_info] = boost::bind(&server::info, this, _1);
		fid_dispatcher[fid_raw_read] = boost::bind(&server::raw_read, this, _1);
		fid_dispatcher[fid_raw_write] = boost::bind(&server::raw_write, this, _1);
		fid_dispatcher[fid_init_m22] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_read_m22] = boost::bind(&server::read_adc, this, _1);
		fid_dispatcher[fid_init_m23] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_read_m23] = boost::bind(&server::read_adc, this, _1);
		fid_dispatcher[fid_init_m26] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_write_m26] = boost::bind(&server::write_dac, this, _1);
		fid_dispatcher[fid_init_m27] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_write_m27] = boost::bind(&server::write_dac, this, _1);
		fid_dispatcher[fid_init_m28] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_read_m28] = boost::bind(&server::read_counters, this, _1);
		fid_dispatcher[fid_init_m32] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_read_m32] = boost::bind(&server::read_counters, this, _1);
		fid_dispatcher[fid_init_m33] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_read_m33] = boost::bind(&server::read_adc, this, _1);
		fid_dispatcher[fid_init_m34] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_read_m34] = boost::bind(&server::read_adc, this, _1);
		fid_dispatcher[fid_init_mmc01] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_write_mmc01] = boost::bind(&server::write_mmc01, this, _1);
		fid_dispatcher[fid_init_mmc03] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_rw_mmc03] = boost::bind(&server::rw_mmc, this, _1);
		fid_dispatcher[fid_init_mmc06] = boost::bind(&server::init_location, this, _1);
		fid_dispatcher[fid_rw_mmc06] = boost::bind(&server::rw_mmc, this, _1);
		fid_dispatcher[fid_another_load_blob] = boost::bind(&server::load_blob, this, _1);

		socket_.async_receive_from(
			boost::asio::buffer(req_data_, max_length),
			sender_endpoint_,
			boost::bind(&server::handle_receive_from, this,
				boost::asio::placeholders::error,
				boost::asio::placeholders::bytes_transferred));
	}

	void
	handle_receive_from(
		const boost::system::error_code& error,
		std::size_t bytes_recvd)
	{
		if (!error && bytes_recvd > 0) {
			socket_.async_send_to(
				boost::asio::buffer(ack_data_,
					fid_dispatcher[req_data_[0]](bytes_recvd)),
				sender_endpoint_,
				boost::bind(&server::handle_send_to, this,
					boost::asio::placeholders::error,
					boost::asio::placeholders::bytes_transferred));
		} else {
			socket_.async_receive_from(
				boost::asio::buffer(req_data_, max_length),
				sender_endpoint_,
				boost::bind(&server::handle_receive_from, this,
					boost::asio::placeholders::error,
					boost::asio::placeholders::bytes_transferred));
		}
	}

	void
	handle_send_to(
		const boost::system::error_code& /*error*/,
		std::size_t /*bytes_sent*/)
	{
		socket_.async_receive_from(
			boost::asio::buffer(req_data_, max_length),
		    sender_endpoint_,
			boost::bind(&server::handle_receive_from, this,
				boost::asio::placeholders::error,
				boost::asio::placeholders::bytes_transferred));
	}

private:
	boost::asio::io_service& io_service_;
	udp::socket socket_;
	udp::endpoint sender_endpoint_;

	enum { max_length = udp_safe_max_size };

	uint8_t req_data_[max_length];
	uint8_t ack_data_[max_length];

	boost::multi_array<uint32_t,2> raw;
	boost::multi_array<uint16_t,2> dac;

	boost::array< boost::function< std::size_t (std::size_t) >, 256 > fid_dispatcher;

	std::vector<std::string> blob;
	std::size_t next_line;

	std::size_t
	unsupported(std::size_t bytes_recvd)
	{
		req_hdr* r = reinterpret_cast<req_hdr*>(req_data_);
		ack_hdr* a = reinterpret_cast<ack_hdr*>(ack_data_);

		a->fid = r->fid;
		a->addr = r->addr;
		a->result = fid_unsupported;

		return sizeof(ack_hdr);
	}

	std::size_t
	info(std::size_t bytes_recvd)
	{
		req_hdr* r = reinterpret_cast<req_hdr*>(req_data_);
		pb_info_ack* a = reinterpret_cast<pb_info_ack*>(ack_data_);

		a->hdr.fid = r->fid;
		a->hdr.addr = r->addr;
		a->hdr.result = success;
		a->data = 0x20120100;

		return sizeof(pb_info_ack);
	}

	std::size_t
	load_blob(std::size_t bytes_recvd)
	{
		pb_load_blob_req_hdr* r = reinterpret_cast<pb_load_blob_req_hdr*>(req_data_);
		pb_load_blob_ack* a = reinterpret_cast<pb_load_blob_ack*>(ack_data_);

		a->fid = r->fid;
		a->nested_fid = r->nested_fid;

		switch  (r->nested_fid) {
		case nested_fid_data:
			if (r->args != next_line) {
				a->result = out_of_order;
			} else {
				static const boost::regex e("^:[[:xdigit:]]+$");
				std::string s;
				const char* d = reinterpret_cast<const char*>(&req_data_[sizeof(pb_load_blob_req_hdr)]);

				s.append(d, bytes_recvd - sizeof(pb_load_blob_req_hdr));

				boost::algorithm::trim(s);
				if (boost::regex_match(s, e))
					blob.push_back(s);

				next_line++;
				a->result = success;
			}
			a->next_line = next_line;
			break;
		case nested_fid_write:
			{
				std::ofstream ofs("emu.hex", std::ios_base::out | std::ios_base::binary | std::ios_base::trunc);

				for (std::vector<std::string>::iterator it = blob.begin();
						it < blob.end(); it++)
					ofs << *it << "\r\n";
				ofs.close();
			}
		case nested_fid_start:
			blob.clear();
			a->next_line = next_line = 0;
			a->result = success;
			break;
		default:
			a->result = fid_unsupported;
		}

		return sizeof(pb_load_blob_ack);
	}

	std::size_t
	raw_read(std::size_t bytes_recvd)
	{
		pb_raw_rw_req* r = reinterpret_cast<pb_raw_rw_req*>(req_data_);
		pb_raw_rw_ack* a = reinterpret_cast<pb_raw_rw_ack*>(ack_data_);

		a->hdr.fid = r->hdr.fid;
		a->hdr.addr = r->hdr.addr;
		a->data = raw[r->hdr.addr.piggyback_location][r->addr & 0x3f];
		a->hdr.result = success;
		return sizeof(pb_raw_rw_ack);
	}

	std::size_t
	raw_write(std::size_t bytes_recvd)
	{
		pb_raw_rw_req* r = reinterpret_cast<pb_raw_rw_req*>(req_data_);
		pb_raw_rw_ack* a = reinterpret_cast<pb_raw_rw_ack*>(ack_data_);

		a->hdr.fid = r->hdr.fid;
		a->hdr.addr = r->hdr.addr;
		raw[r->hdr.addr.piggyback_location][r->addr & 0x3f] = r->data;
		a->data = r->data;
		a->hdr.result = success;
		return sizeof(pb_raw_rw_ack);
	}
	
	std::size_t
	init_location(std::size_t bytes_recvd)
	{
		pb_init_location_req* r = reinterpret_cast<pb_init_location_req*>(req_data_);
		ack_hdr* a = reinterpret_cast<ack_hdr*>(ack_data_);

		a->fid = r->hdr.fid;
		a->addr = r->hdr.addr;
		a->result = success;

		return sizeof(ack_hdr);
	}	

	std::size_t
	read_adc(std::size_t bytes_recvd)
	{
		pb_adc_req* r = reinterpret_cast<pb_adc_req*>(req_data_);
		ack_hdr* a = reinterpret_cast<ack_hdr*>(ack_data_);
		uint16_t* d = reinterpret_cast<uint16_t*>(&ack_data_[sizeof(ack_hdr)]);
		std::size_t len = 0;

		for (std::size_t n = 0; n < 16; n++) {
			if (r->inp_mask & (1<<n))
				d[len++] = dac[r->hdr.addr.piggyback_location][n%4];
		}

		a->fid = r->hdr.fid;
		a->addr = r->hdr.addr;
		a->result = success;

		return sizeof(ack_hdr) + (len * sizeof(uint16_t));
	}

	std::size_t
	write_dac(std::size_t bytes_recvd)
	{
		pb_dac_req* r = reinterpret_cast<pb_dac_req*>(req_data_);
		ack_hdr* a = reinterpret_cast<ack_hdr*>(ack_data_);

		for (std::size_t n = 0; n < 4; n++) {
			if (r->out_mask & (1<<n))
				dac[r->hdr.addr.piggyback_location][n] = r->dout[n];
		}

		a->fid = r->hdr.fid;
		a->addr = r->hdr.addr;
		a->result = success;

		return sizeof(ack_hdr);
	}

	std::size_t
	read_counters(std::size_t bytes_recvd)
	{
		pb_counters_req* r = reinterpret_cast<pb_counters_req*>(req_data_);
		ack_hdr* a = reinterpret_cast<ack_hdr*>(ack_data_);
		uint32_t* d = reinterpret_cast<uint32_t*>(&ack_data_[sizeof(ack_hdr)]);

		std::size_t len = (r->inp_chan == 0xff) ? 4 : 1;
		
		for (std::size_t n = 0; n < 4; n++)
			d[n] = std::rand();

		a->fid = r->hdr.fid;
		a->addr = r->hdr.addr;
		a->result = success;

		return sizeof(ack_hdr) + len * sizeof(uint32_t);
	}

	std::size_t
	write_mmc01(std::size_t bytes_recvd)
	{
		req_hdr* r = reinterpret_cast<req_hdr*>(req_data_);
		ack_hdr* a = reinterpret_cast<ack_hdr*>(ack_data_);

		a->fid = r->fid;
		a->addr = r->addr;
		a->result = success;

		return sizeof(ack_hdr);
	}

	std::size_t
	rw_mmc(std::size_t bytes_recvd)
	{
		pb_mmc_rw_req* r = reinterpret_cast<pb_mmc_rw_req*>(req_data_);
		ack_hdr* a = reinterpret_cast<ack_hdr*>(ack_data_);
		uint16_t* d = reinterpret_cast<uint16_t*>(&ack_data_[sizeof(ack_hdr)]);
		std::size_t len = 0;

		for (std::size_t n = 0; n < 8; n++) {
			if (r->out_mask & (1<<n))
				dac[r->hdr.addr.piggyback_location][n] = r->dout[n];
		}

		for (std::size_t n = 0; n < 32; n++) {
			if (r->inp_mask & (1<<n))
				d[len++] = dac[r->hdr.addr.piggyback_location][n%8];
		}

		a->fid = r->hdr.fid;
		a->addr = r->hdr.addr;
		a->result = success;

		return sizeof(ack_hdr) + len*sizeof(uint16_t);
	}
};
}

int
main(int argc, char* argv[])
{
	try
	{
		if (argc != 2) {
			std::cerr << "Usage: protonos8-emu <port>\n";
			return 1;
		}

		boost::asio::io_service io_service;

		protonos8::server s(io_service, std::atoi(argv[1]));

		io_service.run();
	} catch (std::exception& e) {
		std::cerr << "Exception: " << e.what() << "\n";
	}

	return 0;
}

